/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* This will be a huikea paikannus vekotin 
*/

#include<stdio.h>
#include<stdlib.h>
#include<math.h>
#include "CScanMatchInterface.h"
#include "C3DCorrelationScanMatch.h"
#include "CAHistogramECorrelationScanMatch.h"
#include "CParticleFilter.h"
#include "CLineMapRawScanMCL.h"
#include "CCornerMapMCL.h"

/// For reading the data from file
int readScanLine181(FILE *f,ScanMatch::scan &meas){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f ",&time_s);
	//fprintf(stderr,"1 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f",&meas.a[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"2 ");
		for(i=0;i<181;i++){
				fscanf(f,"%f",&meas.r[i]);
				fscanf(f,"%c",&c);
   // fprintf(stderr,"%.1f ",meas.r[i]);
				if(c=='\n' && i<181){
						fprintf(stderr,"EOL reached at i=%d\n",i);
						meas.N = i;
						return 0;
				}
		}
	//fprintf(stderr,"3 \n");
		meas.N = 181;
		return 0;
}

/// For reading the data from file
/// FIXME!!! The odometry order now heading x y

int readOdometryLine(FILE *f,mcl::pose &odo){
		float time_s;
		int i;
		char c;
		fscanf(f,"%f ",&time_s);
	
		fscanf(f,"%f %f %f\n",&odo.a,&odo.x,&odo.y);
		fscanf(f,"%c",&c);
		
		return 0;
}



int main(void){
		ScanMatch::scan meas;  ///< The Reference scan 
		FILE *f,*out,*fodo;
		CLineMapRawScanMCL mcl,mcl_global;
		
		meas.allocate(181);
		out = fopen("result.txt","wt");
		
		f = fopen("ranging_data2.txt","rt");
		fodo = fopen("odometry_data2.txt","rt");
						
		
		if(f==NULL){
				fprintf(stderr,"Error opening data file\n");
				return 1;
		}
		///The first measurement
  	readScanLine181(f,meas);
 	
		mcl::pose initPose; 
		mcl::pose initVariance;
		mcl::pose dP;
		mcl::pose final;
		mcl::scan measurement;
		mcl::pose variance;
		mcl::pose odo_old;
		mcl::pose odo_cur;
		mcl::pose integral;
		integral.set(0,0,0);
		initPose.set(0,0,0);
		//initVariance.set(1,1,M_PI/2);
		initVariance.set(0.3,0.3,M_PI/8.0);
		measurement.allocate(181);
		
		///Read the first odometry
		readOdometryLine(fodo,odo_old);
		
		mcl.initialize("map.txt",
										1.0, 
										500, 
										initPose, 
										initVariance,
										15,
										-M_PI/2.0-5.0*M_PI/180.0);
		
		FILE *ff = fopen("virtualscan.txt","wt");
		
		float virtualr[181];
		float vlines[3*181];
		
		
		mcl.map.virtualScanner(virtualr, ///<out: distance "measurements"
													 NULL, ///< out: measurements in relative x,y-frame
													 NULL, ///< out: measurements in absolute x,y-frame
													 vlines,  ///< out: The line parameters of the scan
													 0,
													 0,
													 0,
													 181, ///<Number of measurements taken
													 -M_PI/2.0-5.0*M_PI/180.0); ///<offset between laser and robot	meas.a[0] is added since the scan does not began allways from zero
		
		for(int i=0;i<181;i++){
				fprintf(ff,"%.3f ",virtualr[i]);	
		}
			fprintf(ff,"\n");
				
			fclose(ff);
		
		
		dP.set(0.1,0.1,M_PI * 10.0/180.0);
		
		mcl_global.initializeGlobal("map.txt",
												 1.0, 
												 dP, 
												 20,
												 -M_PI/2.0-5.0*M_PI/180.0);
		int counter=0;
		while(!feof(f) && !feof(fodo)){
				
							
				//mcl_global.filt.saveToFile(counter);
				mcl.filt.saveToFile(counter);
												
				readScanLine181(f,meas);
				readOdometryLine(fodo,odo_cur);
				dP.setToDifferentialPose(odo_cur,odo_old);
				fprintf(stderr,"%d:: dP = (%.2f,%.2f,%.2f) Liks=%.2f\n",counter,dP.x,dP.y,dP.a,mcl.filt.Lik);
				odo_old = odo_cur;
				///calculate differential movement ie initial estimate
				measurement.set(meas.r,meas.a);
				measurement.N = meas.N;
				variance.set(0.1,0.1,M_PI/90);
				
				if(counter == 0){
						variance.set(0,0,0);
						dP.set(0,0,0);
				}
				else{
						//Lets assume 50% odometry error
						variance.set(0.02,0.01,M_PI/180);
						variance.set(0.5*dP.x+0.01,0.2*dP.y+0.01,M_PI/180);
						//variance.set(0.02,0.01,M_PI/180);
				}
			
				//mcl_global.runMCLInit(dP,variance, measurement);
				
				mcl.runMCL(dP,variance, measurement);
		
				final = mcl.filt.getDistributionMean();
				//final = mcl_global.filt.getDistributionMean();
				
				fprintf(out,"%.2f %.2F %.2f ",final.x,final.y,final.a);
				//fprintf(out,"%.2f %.2F %.2f ",odo_cur.x,odo_cur.y,odo_cur.a);
				//fprintf(out,"%.2f %.2F %.2f ",integral.x,integral.y,integral.a);
				for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",meas.r[scans]);
				for(int scans=0;scans<meas.N;scans++) fprintf(out,"%.3f ",meas.a[scans]);
				fprintf(out,"\n");
				fflush(out);
				counter++;
		}
		fprintf(stderr,"Reached the end of logfile - Exiting!");
		fclose(out);
   
		return 0;
}
